#include <mc_ros/processor/state_machine.h>

// 初始化状态表
void StateMachineBase::InitStateTable(){
    // ST_IDLE
    // ST_IDLE -> EV_IDLE -> ST_IDLE
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_IDLE, &StateMachineBase::IdleStateHandle,
        StateMachine::Event::EV_IDLE, StateMachine::State::ST_IDLE));  
    // ST_IDLE -> EV_WORKING -> ST_EXECUTING
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_IDLE, &StateMachineBase::IdleStateHandle,
        StateMachine::Event::EV_WORKING, StateMachine::State::ST_EXECUTING)); 

    // ST_EXECUTING
    // ST_EXECUTING -> EV_WORKING -> ST_EXECUTING
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_EXECUTING,&StateMachineBase::ExecutingStateHandle,
        StateMachine::Event::EV_WORKING, StateMachine::State::ST_EXECUTING));
    // ST_EXECUTING -> EV_CANCEL -> ST_CANCEL
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_EXECUTING,&StateMachineBase::ExecutingStateHandle,
        StateMachine::Event::EV_CANCEL, StateMachine::State::ST_CANCEL));
    // ST_EXECUTING -> EV_OVER -> ST_FINISHED
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_EXECUTING,&StateMachineBase::ExecutingStateHandle,
        StateMachine::Event::EV_OVER, StateMachine::State::ST_FINISHED));
    // ST_EXECUTING -> EV_PAUSE -> ST_CANCEL
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_EXECUTING,&StateMachineBase::ExecutingStateHandle,
        StateMachine::Event::EV_PAUSE, StateMachine::State::ST_PAUSE));
    // ST_EXECUTING -> EV_RESUME -> ST_EXECUTING
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_EXECUTING,&StateMachineBase::ExecutingStateHandle,
        StateMachine::Event::EV_RESUME, StateMachine::State::ST_EXECUTING));

    // ST_CANCEL
    // ST_CANCEL -> EV_IDLE -> ST_IDLE
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_CANCEL,&StateMachineBase::CancelStateHandle,
        StateMachine::Event::EV_IDLE, StateMachine::State::ST_IDLE));

    // ST_FINISHED
    // ST_FINISHED -> EV_OVER -> ST_IDLE
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_FINISHED,&StateMachineBase::FinishedStateHandle,
        StateMachine::Event::EV_OVER, StateMachine::State::ST_IDLE));
        
    // ST_PAUSE
    // ST_PAUSE -> EV_PAUSE -> ST_PAUSE
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_PAUSE,&StateMachineBase::PauseStateHandle,
        StateMachine::Event::EV_PAUSE, StateMachine::State::ST_PAUSE));
    // ST_PAUSE -> EV_RESUME -> ST_EXECUTING
    stateTable_.emplace_back(new StateMachine(StateMachine::State::ST_PAUSE,&StateMachineBase::PauseStateHandle,
        StateMachine::Event::EV_RESUME, StateMachine::State::ST_EXECUTING));
}


//状态转移
void StateMachineBase::TransferState(StateMachine::State nex_state){
    LOG(INFO) << "TransferState " << State2Str(currState_) << " -> " << State2Str(nex_state);
    SetAction(StateMachine::Action::AC_ENTRY);
    currState_ = nex_state;   

    if(currEvent_ == StateMachine::Event::EV_CANCEL){
        setEvent(StateMachine::Event::EV_IDLE);
    }
}

// 变更下个状态
void StateMachineBase::ChangeNextState(StateMachine::State nex_state){
    LOG(INFO) << "ChangeNextState " << State2Str(nextState_) << " -> " << State2Str(nex_state);
    nextState_ = nex_state;
}

// 设置动作
void StateMachineBase::SetAction(StateMachine::Action action){
    LOG(INFO) << "SetAction " << Action2Str(currAction_) << " -> " << Action2Str(action);
    currAction_ = action;
}

// 获取当前动作
StateMachine::Action StateMachineBase::GetCurrAction(){
    return currAction_;
}

// 设置事件
void StateMachineBase::setEvent(StateMachine::Event event){
    LOG(INFO) << "setEvent " << Event2Str(currEvent_)  << " -> " << Event2Str(event);
    currEvent_ = event;
}

// 更新状态数据
void StateMachineBase::updateRobotState(geometry_msgs::PoseStamped robot_pose, nav_msgs::Odometry base_odom){
    // robotPoseStamp_ = robot_pose; 
    robotPose_ = trajectoryROS_.toTrajectoryPoint2D(robot_pose);
    baseOdometry_ = base_odom;
}

// 处理事件
void StateMachineBase::handleEvent(){   
    StateMachine::fun_ptr fun_ptr;                    //动作
    bool flag = false;
    for(int i = 0; i < stateTable_.size(); i++){
        if(currEvent_ == stateTable_[i]->currEvent_ && currState_ == stateTable_[i]-> currState_){
            flag = true;
            fun_ptr = stateTable_[i]->__fun_ptr;
            nextState_ = stateTable_[i]->nextState_;
            break;
        }
    }
    //找到对应的状态项，调用对应的动作函数action，然后调用transferState函数更新状态机到新状态
    if(flag){
        if(fun_ptr){
            (this->*fun_ptr)(currAction_);
        }
    }
    else{
        if(currState_ != StateMachine::State::ST_IDLE)
            cout << "can not find state to transfer!" << endl;
    }
}   


// LOG(INFO) << "info test";  //输出一个Info日志
// LOG(WARNING) << "warning test";  //输出一个Warning日志
// LOG(ERROR) << "error test";  //输出一个Error日志
// LOG(FATAL) << "fatal test";  //输出一个Fatal日志，这是最严重的日志并且输出之后会中止程序

// 空闲状态处理
void StateMachineBase::IdleStateHandle(StateMachine::Action& action){

    // AC_ENTRY
    if(action == StateMachine::Action::AC_ENTRY){
        LOG(INFO) << "ST_IDLE | AC_ENTRY";
        smResetIdle();    //IDLE数据重置
        SetAction(StateMachine::Action::AC_RUN);
        return;
    }

    // AC_RUN
    if(action == StateMachine::Action::AC_RUN){
        LOG(INFO) << "ST_IDLE | AC_RUN";
        switch(currEvent_){
            case StateMachine::Event::EV_WORKING:
                SetAction(StateMachine::Action::AC_EXIT);
                break;
            default:
                break;      //保持AC_RUN
        }
        return;
    }

    // AC_EXIT
    if(action == StateMachine::Action::AC_EXIT){
        LOG(INFO) << "ST_IDLE | AC_EXIT";
        TransferState(nextState_);
        return;
    }
}


// 执行状态处理
void StateMachineBase::ExecutingStateHandle(StateMachine::Action& action){

    // AC_ENTRY
    if(action == StateMachine::Action::AC_ENTRY){
        LOG(INFO) << "ST_EXECUTING | AC_ENTRY";
        smExecutingEntry();     // 执行中预处理函数
        SetAction(StateMachine::Action::AC_RUN);
        return;
    }

    // AC_RUN
    if(action == StateMachine::Action::AC_RUN){
        LOG(INFO) << "ST_EXECUTING | AC_RUN";
        switch(currEvent_){
            case StateMachine::Event::EV_WORKING:   //保持AC_RUN
                smExecutingRunning();     // 执行中处理函数
                break;
            case StateMachine::Event::EV_CANCEL:
                SetAction(StateMachine::Action::AC_EXIT);
                break;
            case StateMachine::Event::EV_OVER:
                SetAction(StateMachine::Action::AC_EXIT);
                break;
            case StateMachine::Event::EV_PAUSE:
                SetAction(StateMachine::Action::AC_EXIT);
                break;
            case StateMachine::Event::EV_RESUME:
                ;   //忽略  执行状态忽略恢复事件
                break;
            default:    
                ;
                break;      
        }
        return;
    }

    // AC_EXIT
    if(action == StateMachine::Action::AC_EXIT){
        LOG(INFO) << "ST_EXECUTING | AC_EXIT";
        smExecutingExit();     // 执行中后处理函数
        TransferState(nextState_);
        return;
    }
}


// 取消状态处理
void StateMachineBase::CancelStateHandle(StateMachine::Action& action){

    // AC_ENTRY
    if(action == StateMachine::Action::AC_ENTRY){
        LOG(INFO) << "ST_CANCEL | AC_ENTRY";
        SetAction(StateMachine::Action::AC_RUN);
        return;
    }

    // AC_RUN
    if(action == StateMachine::Action::AC_RUN){
        LOG(INFO) << "ST_CANCEL | AC_RUN";
        smCancelRunning();
        SetAction(StateMachine::Action::AC_EXIT);
    }

    // AC_EXIT
    if(action == StateMachine::Action::AC_EXIT){
        LOG(INFO) << "ST_CANCEL | AC_EXIT";
        TransferState(nextState_);
        return;
    }
}


// 结束状态处理
void StateMachineBase::FinishedStateHandle(StateMachine::Action& action){

    // AC_ENTRY
    if(action == StateMachine::Action::AC_ENTRY){
        LOG(INFO) << "ST_FINISHED | AC_ENTRY";
        SetAction(StateMachine::Action::AC_RUN);
        return;
    }

    // AC_RUN
    if(action == StateMachine::Action::AC_RUN){
        LOG(INFO) << "ST_FINISHED | AC_RUN";
        smFinishedRunning();
        SetAction(StateMachine::Action::AC_EXIT);
    }

    // AC_EXIT
    if(action == StateMachine::Action::AC_EXIT){
        LOG(INFO) << "ST_FINISHED | AC_EXIT";
        TransferState(nextState_);
        return;
    }
}


// 暂停状态处理
void StateMachineBase::PauseStateHandle(StateMachine::Action& action){
    // AC_ENTRY
    if(action == StateMachine::Action::AC_ENTRY){
        LOG(INFO) << "ST_PAUSE | AC_ENTRY";
        SetAction(StateMachine::Action::AC_RUN);
        return;
    }

    // AC_RUN
    if(action == StateMachine::Action::AC_RUN){
        LOG(INFO) << "ST_PAUSE | AC_RUN";
        switch(currEvent_){
            case StateMachine::Event::EV_PAUSE:
                smPauseRunning();     // 执行中处理函数
                break;
            case StateMachine::Event::EV_RESUME:
                SetAction(StateMachine::Action::AC_EXIT);
                break;
            default:    //保持AC_RUN
                break;      
        }
        return;
    }

    // AC_EXIT
    if(action == StateMachine::Action::AC_EXIT){
        LOG(INFO) << "ST_PAUSE | AC_EXIT";
        TransferState(nextState_);
        setEvent(StateMachine::Event::EV_WORKING);      //恢复执行状态
        return;
    }    
}